/*****************************************************************************
*
* CPE439
*
* Final Project.
*
* File: main.c
*
* Description: RTOS-based application that:
*			      -Emulates snake game on 3D LED matrix.
*
* Target Device: ATmega32 on STK500 development board.
*
* Hardware setup: STK500 board with ATmega32 MCU.
*				  6x4x4 LED cube
*                 PS/2 keyboard
*				  
*                 -PORTA, PORTC and PORTD(PIN2 - PIN7) used to drive 
*                  LED cube.
*                 -PS/2 keyboard connected to UART port. 
*                  PB0 for XCK and PD0 for RXD
*           
* Author: Kevin Casteel, Luis Castillo
*
* Revisions: v-1.0 (5/25/11)
*			 
******************************************************************************/
#define F_CPU 3686400
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <util/atomic.h>
#include "FreeRTOS.h"
#include "semphr.h"
#include "task.h"
#include "queue.h"
#include "hw_init.h"

//- variable declaration
xSemaphoreHandle xBinary1;

//- task handle declarations
xTaskHandle xHandle1;
xTaskHandle xHandle2;
xTaskHandle xHandle3;

//- declaration of global variables
volatile uint8_t g_Dead = 0;
volatile uint8_t g_count = 0;
volatile uint8_t g_udr;
volatile uint8_t g_snake_len = 1;
volatile uint8_t g_direction[6] = {HEAVEN};
volatile location_t g_snake[6];
volatile cube_t g_cube;
volatile uint16_t g_delay = FAST;



/*---------------------------------------------------------
* Function: main
*
* Description: Calls functions to initialize IO, creates
* tasks, and starts up the scheduler.
*
* return: 0 on success, non-zero value on failure.
*---------------------------------------------------------*/
int main( void )
{
	//- initialize things
	vInit_HW();
	vInit_Test();
	vUSART_Init();
	vInitSnake(g_snake);
	vClearCube(&g_cube);


	//- create and immediately take binary semaphore
	vSemaphoreCreateBinary(xBinary1);
	xSemaphoreTake(xBinary1, portMAX_DELAY);

	
	//- create tasks
	xTaskCreate(vReadKBTask, "KBHandler", configMINIMAL_STACK_SIZE, 
				NULL, 3, &xHandle1 );
	
	xTaskCreate(vMotionTask, "MoveSnake", configMINIMAL_STACK_SIZE, 
				NULL, 2, &xHandle2 );
	
	xTaskCreate(vGrowSnakeTask, "GrowSnake", configMINIMAL_STACK_SIZE, 
				NULL, 2, &xHandle3 );
    
	xTaskCreate(vCheckDeath, "CheckDeath", configMINIMAL_STACK_SIZE, 
				NULL, 2, NULL );


    //- kick off the scheduler
	vTaskStartScheduler();

	return 0;
}


/************************************************ 
 *
 * Function: vReadKBTask()
 *
 * Description: Reads arrow keys on keyboard.
 *
 ************************************************/
void vReadKBTask(void *pvParameters)
{
	volatile int i;

	for(;;)
	{
		//- take semaphore, given from ISR
		xSemaphoreTake(xBinary1, portMAX_DELAY);

		if((g_count%5)==0)
		{
			ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
			{
				//- transfer direction to next member
				for(i=(MAXSNAKELENGTH-1);i>0;i--)
				{
					g_direction[i] = g_direction[i-1];
				}


				//- determine new direction based on
				//- previous direction			
				if(g_direction[1] == NORTH) //- previous direction
				{
					//- based on key pressed,
					//- determine new direction
					switch(g_udr)
					{
						case UP:
							g_direction[0] = HEAVEN;
							break;
						case DOWN:
							g_direction[0] = HELL;
							break;
						case RIGHT:
							g_direction[0] = EAST;
							break;
						case LEFT:
							g_direction[0] = WEST;
							break;
						default:
							break;
					}
				}
				else if(g_direction[1] == SOUTH) //- previous direction
				{
					//- based on key pressed,
					//- determine new direction
					switch(g_udr)
					{
						case UP:
							g_direction[0] = HEAVEN;
							break;
						case DOWN:
							g_direction[0] = HELL;
							break;
						case RIGHT:
							g_direction[0] = WEST;
							break;
						case LEFT:
							g_direction[0] = EAST;
							break;
						default:
							break;
					}
				}
				else if(g_direction[1] == EAST) //- previous direction
				{
					//- based on key pressed,
					//- determine new direction
					switch(g_udr)
					{
						case UP:
							g_direction[0] = HEAVEN;							
							break;
						case DOWN:
							g_direction[0] = HELL;
							break;
						case RIGHT:
							g_direction[0] = SOUTH;
							break;
						case LEFT:
							g_direction[0] = NORTH;
							break;
						default:
							break;
					}
				}
				else if(g_direction[1] == WEST) //- previous direction
				{
					//- based on key pressed,
					//- determine new direction
					switch(g_udr)
					{
						case UP:
							g_direction[0] = HEAVEN;							
							break;
						case DOWN:
							g_direction[0] = HELL;
							break;
						case RIGHT:
							g_direction[0] = NORTH;
							break;
						case LEFT:
							g_direction[0] = SOUTH;
							break;
						default:
							break;
					}
				}
				else if(g_direction[1] == HEAVEN) //- previous direction
				{
					//- based on key pressed,
					//- determine new direction
					switch(g_udr)
					{
						case UP:
							g_direction[0] = NORTH;							
							break;
						case DOWN:
							g_direction[0] = SOUTH;
							break;
						case RIGHT:
							g_direction[0] = EAST;
							break;
						case LEFT:
							g_direction[0] = WEST;
							break;
						default:
							break;
					}
				}
				else if(g_direction[1] == HELL) //- previous direction
				{
					//- based on key pressed,
					//- determine new direction
					switch(g_udr)
					{
						case UP:
							g_direction[0] = NORTH;							
							break;
						case DOWN:
							g_direction[0] = SOUTH;
							break;
						case RIGHT:
							g_direction[0] = EAST;
							break;
						case LEFT:
							g_direction[0] = WEST;
							break;
						default:
							break;
					}
				}

				//- after new direction is updated
				//- call functions to move snake
				//- and display to LED cube
				vMoveSnake();
				vTranslateLocation(&g_cube);
				vSetCubeLighting(&g_cube);
			
			}//- atomic block
		}//- if statement		
	}//- infinite loop
}


/************************************************ 
 *
 * Function: vMotionTask()
 *
 * Description: Calls function to move and
 *              display snake.
 *
 ************************************************/
void vMotionTask(void *pvParameters)
{
	for(;;)
	{
		//- task periodically calls
		//- functions to move snake
		//- and display to LED cube
		vMoveSnake();
		ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
		{
			vTranslateLocation(&g_cube);
		}
		vSetCubeLighting(&g_cube);

		//- delay controls how fast 
		//- the snake moves
		_delay_ms(g_delay);
	}
}


/************************************************ 
 *
 * Function: vGrowSnakeTask()
 *
 * Description: Increases snake length after
 *              a set time.
 *
 ************************************************/
void vGrowSnakeTask(void *pvParameters)
{
	volatile int i;
	
	for(;;)
	{
		//- automatically grow snake
		//- after delay
		_delay_ms(DELAY);

		ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
		{
			//- increase snake length
			if(g_snake_len < MAXSNAKELENGTH)
			{
				g_snake_len++;
			}
			//- if snake length is 6
			//- reset to 1 and increase
			//- frequency of snake motion
			else 
			{
				g_snake_len = 1;
				g_delay = SUPERFAST;
			}

			//- shift members of snake body array
			for(i=(g_snake_len-1);i>0;i--)
			{
				g_snake[i] = g_snake[i-1];
			}

			//- shift data in direction array
			for(i=(MAXSNAKELENGTH-1);i>0;i--)
			{
				g_direction[i] = g_direction[i-1];
			}

			//- snake extends from head end
			//- based on current direction
			//- of the head
			switch(g_direction[0])
			{
				case NORTH:
					{
						g_snake[0].row--;
						if(g_snake[0].row == 255)
						{
							g_snake[0].row = CUBEWIDTH - 1;
						}
					}
					break;
				case SOUTH:
					{
						g_snake[0].row++;
						if(g_snake[0].row == CUBEWIDTH)
						{
							g_snake[0].row = 0;
						}
					}
					break;
				case EAST:
					{
						g_snake[0].col++;
						if(g_snake[0].col == CUBELENGTH)
						{
							g_snake[0].col = 0;
						}
					}
					break;
				case WEST:
					{
						g_snake[0].col--;
						if(g_snake[0].col == 255)
						{
							g_snake[0].col = CUBELENGTH - 1;
						}
					}
					break;
				case HEAVEN:
					{
						g_snake[0].layer++;
						if(g_snake[0].layer == CUBEHEIGHT)
						{
							g_snake[0].layer = 0;
						}
					}
					break;
				case HELL:
					{
						g_snake[0].layer--;
						if(g_snake[0].layer == 255)
						{
							g_snake[0].layer = CUBEHEIGHT - 1;
						}
					}
					break;
				default:
					break;
			}//- switch

			//- check if head has touched body
			for(i = 1; i < g_snake_len; i++)
			{
				if((g_snake[0].layer == g_snake[i].layer) &&
					(g_snake[0].row == g_snake[i].row) &&
					(g_snake[0].col == g_snake[i].col))
				{
					g_Dead = 1;
				}
			}
		}//- atomic block
	}//- infinite loop
}


/************************************************ 
 *
 * Function: vSetCubeLighting()
 *
 * Description: Illuminates the cube according to 
 *				the data stored in current.
 *
 * Param: current - current cube status.
 *
 ************************************************/
void vSetCubeLighting(cube_t *current) {
	volatile uint8_t i, j, k;

	//- loop through each layer
	//- to light up
	for (i=0; i<CUBEHEIGHT;i++) {

		PORTA = PORTC = PORTD = 0;

		switch(i) {
			case 0:
				PORTD = FIRSTLAYER;
				break;
			case 1:
				PORTD = SECONDLAYER;
				break;
			case 2:
				PORTD = THIRDLAYER;
				break;
			case 3:
				PORTD = FOURTHLAYER;
				break;
			case 4:
				PORTD = FIFTHLAYER;
				break;
			case 5:
				PORTD = SIXTHLAYER;
				break;
			default:
				PORTD = 0;
				break;
		}

		ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
		{
			//- for each layer go throw each
			//- row and column and find which
			//- LEDs need to be on
			for(j=0; j<CUBEWIDTH;j++) {
				for(k=0; k<CUBELENGTH;k++) {
					if (current->layers[i].leds[j][k] == 1) {
						switch(j) {
							case 0:
								PORTC |= xFindLED(k) << 4;
								break;
							case 1:
								PORTC |= xFindLED(k);
								break;
							case 2:
								PORTA |= xFindLED(k) << 4;
								break;
							case 3:
								PORTA |= xFindLED(k);
								break;
							default:
								break;
						}


					}
				}
				_delay_ms(1);
			}
		}
	}
	PORTA = PORTC = PORTD = 0;
}


/************************************************ 
 *
 * Function: vTranslateLocation()
 *
 * Description: Converts snake's current location 
 *				to the cube so it can be displayed
 *
 * Parameters: current - Pointer to cube.
 *
 ************************************************/
 void vTranslateLocation(cube_t *current) {
	volatile uint8_t i;
	
	vClearCube(current);

	ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
	{
		//- for each member of snake
		//- translace location_t to cube_t
		//- and assert ocupied cells in cube_t
		//- structure
		for (i=0;i<g_snake_len;i++){
			current->layers[g_snake[i].layer].leds[g_snake[i].row][g_snake[i].col] = 1;
		}
	}
 }
 

/************************************************ 
 *
 * Function: vCheckDeath()
 *
 * Description: Monitors the Death flag so that
 *				game will end when dead.
 *
 ************************************************/
void vCheckDeath() {
	
	for(;;) {
		//- check dead flag
		if (g_Dead == 1) {

			//- if flag set
			//- delete other tasks
			vTaskDelete(xHandle1);
			vTaskDelete(xHandle2);
			vTaskDelete(xHandle3);

			//- call "game over" animation
			vYouDead();
		}
		_delay_ms(200);
	}
}


/************************************************ 
 *
 * Function: xFindLED()
 *
 * Description: Finds LED that needs to be ON.
 *
 * Return: bit value that needs to be asserted.
 *
 ************************************************/
uint8_t xFindLED(uint8_t pos) {
	switch(pos) {
		case 0:
			return _BV(3);
		case 1:
			return _BV(2);
		case 2:
			return _BV(1);
		case 3:
			return _BV(0);
	}
	
	return 0;
}


/************************************************ 
 *
 * Function: vYouDead()
 *
 * Description: Plays animation when game over.
 *
 ************************************************/
void vYouDead() {
	layer_t layer;
	volatile uint8_t i, j;

	for (i=0;i<CUBEWIDTH;i++) 
		for (j=0;j<CUBELENGTH;j++)
			layer.leds[i][j] = 0;
	
	//Begin Animation
	PORTD = SIXTHLAYER;
	PORTC = _BV(1);//1
	_delay_ms(50);

	PORTC |= _BV(2);//2
	_delay_ms(50);

	PORTA = _BV(6);//3
	_delay_ms(50);

	PORTA |= _BV(5);//4
	_delay_ms(50);
	
	PORTA |= _BV(4);//5
	_delay_ms(50);

	PORTC |= _BV(0);//6
	_delay_ms(50);

	PORTC |= _BV(4);//7
	_delay_ms(50);

	PORTC |= _BV(5);//8
	_delay_ms(50);

	PORTC |= _BV(6);//9
	_delay_ms(50);

	PORTC |= _BV(7);//10
	_delay_ms(50);

	PORTC |= _BV(3);//11
	_delay_ms(50);

	PORTA |= _BV(7);//12
	_delay_ms(50);

	PORTA |= _BV(3);//13
	_delay_ms(50);

	PORTA |= _BV(2);//14
	_delay_ms(50);

	PORTA |= _BV(1);//15
	_delay_ms(50);

	PORTA |= _BV(0);//16
	_delay_ms(50);
	
	PORTD |= FIFTHLAYER;
	_delay_ms(50);
	
	PORTD |= FOURTHLAYER;
	_delay_ms(50);
	
	PORTD |= THIRDLAYER;
	_delay_ms(50);
	
	PORTD |= SECONDLAYER;
	_delay_ms(50);
	
	PORTD |= FIRSTLAYER;
	
	//Game Over...
	while(1) {;}
}

 
/************************************************ 
 *
 * Function: vMoveSnake()
 *
 * Description: Updates de location of the snake.
 *
 ************************************************/
void vMoveSnake()
{
	volatile int i;

	ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
	{
		//- loop through each member of snake
		for(i = 0;i < g_snake_len; i++)
		{
			//- move each snake member based
			//- on its own direction member
			switch(g_direction[i])
			{
				case NORTH:
					{
						g_snake[i].row--;

						//- allow snake to wrap around cube
						if(g_snake[i].row == 255)
						{
							g_snake[i].row = CUBEWIDTH - 1;
						}
					}
					break;
				case SOUTH:
					{
						g_snake[i].row++;

						//- allow snake to wrap around cube
						if(g_snake[i].row == CUBEWIDTH)
						{
							g_snake[i].row = 0;
						}
					}
					break;
				case EAST:
					{
						g_snake[i].col++;

						//- allow snake to wrap around cube
						if(g_snake[i].col == CUBELENGTH)
						{
							g_snake[i].col = 0;
						}
					}
					break;
				case WEST:
					{
						g_snake[i].col--;

						//- allow snake to wrap around cube
						if(g_snake[i].col == 255)
						{
							g_snake[i].col = CUBELENGTH - 1;
						}
					}
					break;
				case HEAVEN:
					{
						g_snake[i].layer++;

						//- allow snake to wrap around cube
						if(g_snake[i].layer == CUBEHEIGHT)
						{
							g_snake[i].layer = 0;
						}
					}
					break;
				case HELL:
					{
						g_snake[i].layer--;

						//- allow snake to wrap around cube
						if(g_snake[i].layer == 255)
						{
							g_snake[i].layer = CUBEHEIGHT - 1;
						}
					}
					break;
				default:
					break;
			}//- switch
		}//- for loop

		//- shift directions in array
		for(i=(MAXSNAKELENGTH-1);i>0;i--)
		{
			g_direction[i] = g_direction[i-1];
		}

		//- check if head has touched body
		for(i = 1; i < g_snake_len; i++)
		{
			if((g_snake[0].layer == g_snake[i].layer) &&
				(g_snake[0].row == g_snake[i].row) &&
				(g_snake[0].col == g_snake[i].col))
			{
				g_Dead = 1;
			}
		}
	}
}


/************************************************ 
 *
 * Function: ISR
 *
 * Description: Services USART RXC interrupt.
 *
 * Parameters: USART_RXC_vect interrupt vector.
 *
 ************************************************/
ISR(USART_RXC_vect)
{
	//- interrupt counter
	//- each key press for some reason
	//- gives 5 interrupt pulses
	g_count++;

	//- read data from UART
	g_udr = UDR;

	static portBASE_TYPE xHigherPriorityTaskWoken;
	
	xHigherPriorityTaskWoken = pdFALSE;
	
	//- wake-up task to read key pressed
	xSemaphoreGiveFromISR(xBinary1, &xHigherPriorityTaskWoken);

	if(xHigherPriorityTaskWoken == pdTRUE)
	{
		vPortYield();
	}
}

